#!/usr/bin/env python3
# -*- coding:utf-8 -*-
# @Author: renjin@bit.edu.cn
# @Date  : 2024-11-18
import json

import cv2

from spirems import Publisher, Subscriber, def_msg, Rate, QoS
import threading
import time
import random
from spirems import Logger


logger = Logger()
pub = Publisher('/hello/speed', 'std_msgs::NumberMultiArray', qos=QoS.Reliability)
cnt = 1
max_ms = 0
rate = Rate(200)

while True:
    try:
        rate.sleep()
        tpc = def_msg('std_msgs::NumberMultiArray')
        tpc['data'] = [random.random() for i in range(1000)]
        t1 = time.time()
        # print(len(json.dumps(tpc)))
        pub.publish(tpc)
        t2 = time.time()
        if t2 - t1 > max_ms:
            max_ms = t2 - t1
            # print("sender_max_speed_test - timeout: {}".format(max_ms))
            logger.warn("sender_max_speed_test - timeout: {}".format(max_ms))
    except KeyboardInterrupt:
        pub.kill()
        logger.quit()
        break
